#include "MPointCloudMap.h"
#include "config.h"
#include <pcl/io/pcd_io.h>

#include <cmath>
#include<fstream>
#include<cstring>
#include<stdint.h>
#include<iostream>
#include<string>
#include<pcl/filters/voxel_grid.h>
#include<pcl/registration/transformation_estimation_svd.h>
#include<pcl/registration/icp.h>
#include <pcl/correspondence.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/correspondence_types.h>
//#include <pcl/common/eigen.h>
#include<Eigen/Eigen>
#include<ctime>
#include"LogCat.h"

void MPointCloudMap::setKdtree(const pcl::PointCloud<PointIoType>::Ptr map)
{
	mKdtree.setInputCloud(map);
}

pcl::PointCloud<PointIoType>::Ptr MPointCloudMap::initMap(const string bin_input_path)
{
	MyMap = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
	/*if (pcl::io::loadPCDFile<PointIoType>(POINT_MAP_PATH, *MyMap) == -1) {
		PCL_ERROR("Could not load the pcd file, maybe you should check the path");
		return nullptr;
	}*/
	//getFromBin();
	getFromBin(bin_input_path);
	pcl::VoxelGrid<PointIoType> filter;
	filter.setInputCloud(MyMap);
	filter.setLeafSize(0.3f, 0.3f, 0.3f);
	filter.filter(*MyMap);
	setKdtree(MyMap);
	return 0;
}
pcl::PointCloud<PointIoType>::Ptr MPointCloudMap::getFromBin(const string bin_input_path)
{
	
	string inputfilename = bin_input_path;
	const char* inptfile = inputfilename.data();
	int32_t num = 1000000*100;
	float *data = (float*)malloc(num * sizeof(float));
	// 
	float *px = data + 0;
	float *py = data + 1;
	float *pz = data + 2;
	float *pr = data + 3;
	// 
	FILE *stream;
	fopen_s(&stream, inptfile, "rb");
	num = fread(data, sizeof(float), num, stream) / 4;//
	fclose(stream);
	if (num == 0)
		return nullptr;
	MyMap->resize(num);
	PointIoType point;
	for (int32_t i = 0; i < num; i++)
	{
		point.x = *px;
		point.y = *py;
		point.z = *pz;
		//we do not use the intensity .
		//if should use change the pointIoType and add the code: point.intensity = *pr;
		MyMap->points[i] = point;
		px += 4; py += 4; pz += 4; pr += 4;
	}
	free(data);
	return MyMap;
}



//the observation's center is (0,0,0 )
double MPointCloudMap::likelihoodCompute(const Pose3D & pose, const pcl::PointCloud<PointIoType>::Ptr observation, bool whether_cut)
{
	if (observation) {
		std::vector<int> pointIdNsearch(1);
		std::vector<float> pointSquaredDistance(1);
		
		double sum_distance = 0.0;
		double sigma_dist = 0.2*0.2;
		const double max_sqr_err = pow(4,2);
		size_t counter =0 ;
		for (int n = 0; n < observation->size();n=n+200,counter++) {
			//simulate the pointcloud transform local to global ,because not consider the rotation
			PointIoType global_point;
			if (whether_cut) {
				if (observation->points[n].x > cut_range_x || observation->points[n].y > cut_range_y)
					continue;
			}
			pose.transformPoint(observation->points[n].x, observation->points[n].y, observation->points[n].z
				,global_point.x, global_point.y, global_point.z);
			if (mKdtree.nearestKSearch(global_point, 1, pointIdNsearch, pointSquaredDistance) > 0) {
				//consider the way of compute whether influence the particle weight!!!!!!!!!!!!!!!!!!!!
				sum_distance += std::min<double>(max_sqr_err, pointSquaredDistance[0]);
			}
		}
		if (counter) sum_distance /= counter;
		// -1 is make sure the average distance is nagative number
		return -sum_distance / sigma_dist -1;
	}

	return -1.0;
}

double MPointCloudMap::likelihoodComputeWithPointCloudSave(const Pose3D & pose, const pcl::PointCloud<pcl::PointXYZ>::Ptr observation, bool whether_cut)
{
	if (observation) {
		std::vector<int> pointIdNsearch(1);
		std::vector<float> pointSquaredDistance(1);
		pcl::PointCloud<pcl::PointXYZ>::Ptr source_corr(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr target_corr(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr _observation(observation);

		double sum_distance = 0.0;
		double sigma_dist = 0.2*0.2;
		const double max_sqr_err = pow(4, 2);
		size_t counter = 0;
		for (int n = 0; n < observation->size(); n = n + 200, counter++) {
			//simulate the pointcloud transform local to global ,because not consider the rotation
			PointIoType global_point;
			if (whether_cut) {
				if (observation->points[n].x > cut_range_x || observation->points[n].y > cut_range_y)
					continue;
			}
			pose.transformPoint(observation->points[n].x, observation->points[n].y, observation->points[n].z
				, global_point.x, global_point.y, global_point.z);
			if (mKdtree.nearestKSearch(global_point, 1, pointIdNsearch, pointSquaredDistance) > 0) {
				if (pointSquaredDistance[0] < max_sqr_err) {
					//now get the correspondense point cloud
					source_corr->points.push_back(global_point);
					target_corr->points.push_back(MyMap->at(pointIdNsearch[0]));
				}
				sum_distance += std::min<double>(max_sqr_err, pointSquaredDistance[0]);
			}
		}
		Correspondence_PointCloud corr;
		corr.source = source_corr;
		corr.target = target_corr;
		m_corr.push_back(corr);
		if (counter) sum_distance /= counter;
		// -1 is make sure the average distance is nagative number
		return -sum_distance / sigma_dist - 1;
	}

	return -1.0;
}

Pose3D MPointCloudMap::doComputeWithParticleShift(const Pose3D & pose,const int& index,
	pcl::PointCloud<pcl::PointXYZ>::Ptr observation,
	const int & shift_particle_function)
{
	Eigen::Matrix4f transformation_matrix;
	computeParticleShift(observation, m_corr[index].source, m_corr[index].target, transformation_matrix, shift_particle_function);
	Pose3D pose_return(pose);
	pose_return.transformSlfe(transformation_matrix);
	//cout << abs(pose.x - pose_return.x) << " " << abs(pose.y - pose_return.y)<<" " <<abs(pose.z - pose_return.z)<<endl;
	//pose_return.fromROT4();
	return pose_return;
}



Pose3D MPointCloudMap::likelihoodComputeWithParticleShift(const Pose3D & pose, 
	const pcl::PointCloud<pcl::PointXYZ>::Ptr observation,
	const int& shift_particle_function,bool whether_cut)
{
	if (observation) {
		std::vector<int> pointIdNsearch(1);
		std::vector<float> pointSquaredDistance(1);
		double sum_distance = 0.0;
		double sigma_dist = 0.2*0.2;
		const double max_sqr_err = pow(5, 2);
		size_t counter = 0;
		
		
		
		pcl::PointCloud<pcl::PointXYZ>::Ptr source_corr(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr target_corr(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZ>::Ptr _observation(observation);

		for (int n = 0; n < observation->size(); n = n + 200, counter++) {
			//simulate the pointcloud transform local to global ,because not consider the rotation
			PointIoType global_point;
			if (whether_cut) {
				if (observation->points[n].x > cut_range_x || observation->points[n].y > cut_range_y)
					continue;
			}
			pose.transformPoint(observation->points[n].x, observation->points[n].y, observation->points[n].z
				, global_point.x, global_point.y, global_point.z);
			if (mKdtree.nearestKSearch(global_point, 1, pointIdNsearch, pointSquaredDistance) > 0) {
				if (pointSquaredDistance[0]<max_sqr_err) {	
					//now get the correspondense point cloud
					source_corr->points.push_back(global_point);
					target_corr->points.push_back(MyMap->at(pointIdNsearch[0]));
				}
				sum_distance += std::min<double>(max_sqr_err, pointSquaredDistance[0]);
			}
			//save the point cloud that be computed
			_observation->points.push_back(observation->at(n));
		}
		Eigen::Matrix4f transformation_matrix;
		computeParticleShift(_observation,source_corr,target_corr, transformation_matrix, shift_particle_function);

		Pose3D pose_return(pose);
		pose_return.transformSlfe(transformation_matrix);
		//pose_return.fromROT4();
		return pose_return;
		/*if (counter) sum_distance /= counter;*/
		// -1 is make sure the average distance is nagative number
		//return -sum_distance / sigma_dist - 1;
	}
	return Pose3D();
}

void MPointCloudMap::computeParticleShift(pcl::PointCloud<pcl::PointXYZ>::Ptr observation, 
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_corr,
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_corr, Eigen::Matrix4f & transformation_matrix,
	const int& shift_particle_function)
{
	computeShift(source_corr, target_corr, transformation_matrix, shift_particle_function);
}

void MPointCloudMap::clearCorrespondenceCloudPoint()
{
	this->m_corr.clear();
}

void MPointCloudMap::computeShift(const pcl::PointCloud<pcl::PointXYZ>::Ptr source,
	const pcl::PointCloud<pcl::PointXYZ>::Ptr target,
	Eigen::Matrix4f & transformation_matrix, const int& shift_particle_function)
{
	/*pcl::registration::TransformationEstimationSVD<pcl::PointCloud<pcl::PointXYZ>,
		pcl::PointCloud<pcl::PointXYZ>> mTransformSVD;*/
	//mTransformSVD.estimateRigidTransformation(source, target,transformation_matrix);
	size_t npts;
	if (!source || !target)
		return ;
	if (source->size() == 0)
		return ;

	npts = source->size();
	Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src(3, npts);
	Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt(3, npts);

	double mean_x=0.0;
	double mean_y=0.0;
	double mean_z=0.0;

	switch (shift_particle_function)
	{
	case 0: {
		for (int i = 0; i < npts; ++i)
		{
			cloud_src(0, i) = source->points[i].x;
			cloud_src(1, i) = source->points[i].y;
			cloud_src(2, i) = source->points[i].z;

			cloud_tgt(0, i) = target->points[i].x;
			cloud_tgt(1, i) = target->points[i].y;
			cloud_tgt(2, i) = target->points[i].z;
		}
		// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
		transformation_matrix = pcl::umeyama(cloud_src, cloud_tgt, false);
	}break;
	case 1: {
		for (int i = 0; i < npts; ++i)
		{
			cloud_src(0, i) = source->points[i].x;
			cloud_src(1, i) = source->points[i].y;
			cloud_src(2, i) = source->points[i].z;

			cloud_tgt(0, i) = target->points[i].x;
			cloud_tgt(1, i) = target->points[i].y;
			cloud_tgt(2, i) = target->points[i].z;

			//another shift 
			mean_x += (source->points[i].x - target->points[i].x);
			mean_y += (source->points[i].y - target->points[i].y);
			mean_z += (source->points[i].z - target->points[i].z);
		}
		mean_x = mean_x / npts;
		mean_y = mean_y / npts;
		mean_z = mean_z / npts;

		transformation_matrix(0, 3) = transformation_matrix(0, 3) - mean_x;
		transformation_matrix(1, 3) = transformation_matrix(1, 3) - mean_y;
		transformation_matrix(2, 3) = transformation_matrix(2, 3) - mean_z;

		transformation_matrix(0, 0) = 1;
		transformation_matrix(1, 1) = 1;
		transformation_matrix(2, 2) = 1;
		transformation_matrix(3, 3) = 1;

		transformation_matrix(0, 1) = transformation_matrix(1, 0) =0;
		transformation_matrix(0, 2) = transformation_matrix(2, 0) =0;

		transformation_matrix(2, 1) = transformation_matrix(1, 2) =0;
		transformation_matrix(3, 0) = transformation_matrix(3, 1) = transformation_matrix(3, 2) = 0;
	}break;
	case 2: {
		for (int i = 0; i < npts; ++i)
		{
			cloud_src(0, i) = source->points[i].x;
			cloud_src(1, i) = source->points[i].y;
			cloud_src(2, i) = source->points[i].z;

			cloud_tgt(0, i) = target->points[i].x;
			cloud_tgt(1, i) = target->points[i].y;
			cloud_tgt(2, i) = target->points[i].z;

			//another shift 
			mean_x += (source->points[i].x - target->points[i].x);
			mean_y += (source->points[i].y - target->points[i].y);
			mean_z += (source->points[i].z - target->points[i].z);
		}
		mean_x = mean_x / npts;
		mean_y = mean_y / npts;
		mean_z = mean_z / npts;

		// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
		transformation_matrix = pcl::umeyama(cloud_src, cloud_tgt, true);
		transformation_matrix(0, 3) = transformation_matrix(0, 3) - mean_x;
		transformation_matrix(1, 3) = transformation_matrix(1, 3) - mean_y;
		transformation_matrix(2, 3) = transformation_matrix(2, 3) - mean_z;
	}break;
	default:
		break;
	}
	
	
}


pcl::PointCloud<PointIoType>::Ptr MPointCloudMap::getMapData()
{
	return MyMap;
}

//not use
double MPointCloudMap::europeanDistance(const PointIoType & a, const PointIoType & b)
{
	//consider the way of compute whether influence the particle weight!!!!!!!!!!!!!!!!!!!! 
	return sqrt((a.x-b.x)*(a.x - b.x)
		+ (a.y - b.y)*(a.y - b.y)
		+ (a.z - b.z)*(a.z - b.z));
}



MPointCloudMap::MPointCloudMap()
{

}


MPointCloudMap::~MPointCloudMap()
{
}
